{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "import pandas as pd\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import time\n",
    "from PIL import Image\n",
    "from PIL import ImageDraw\n",
    "import math\n",
    "plt.style.use({'figure.figsize':(10, 10)})"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Epsilon_start=1\n",
    "Epsilon_final=0.01\n",
    "Robot_radium=40\n",
    "Decay_rate=0.000001#he dacaying rate of the Epsilon, the range of the epsilon is 0.01-1, initially it is 1.\n",
    "Action_times=0 #Rigister the totality of the times of selecting actions, including the random selections and selection based on Q_Table\n",
    "Velocity_tripod=0.289*Robot_radium\n",
    "Beta=0.9\n",
    "Alpha=0.2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Q_table2_states=np.array(['L60D0/30','L180D0/30','L250D0/30','L300D0/30',\n",
    "                'L60D30/60','L180D30/60','L250D30/60','L300D30/60',\n",
    "                 'L60D60/90','L180D60/90','L250D60/90','L300D60/90',\n",
    "                'L60D90/120','L180D90/120','L250D90/120','L300D90/120',\n",
    "                'L60D120/150','L180D120/150','L250D120/150','L300D120/150',\n",
    "                'L60D150/180','L180D150/180','L250D150/180','L300D150/180',\n",
    "                'L60D180/210','L180D180/210','L250D180/210','L300D180/210',\n",
    "                'L60D210/240','L180D210/240','L250D210/240','L300D210/240',\n",
    "                'L60D240/270','L180D240/270','L250D240/270','L300D240/270',\n",
    "                'L60D270/300','L180D270/300','L250D270/300','L300D270/300',\n",
    "                'L60D300/330','L180D300/330','L250D300/330','L300D300/330',\n",
    "                 'L60D330/360','L180D330/360','L250D330/360','L300D330/360'])\n",
    "Q_table2_action=np.array(['Up','Left_45D','Right_45D'])\n",
    "Q_table2_actions_length=len(Q_table2_action)\n",
    "Q_table2_states_length=len(Q_table2_states)\n",
    "Q_table2_real=np.zeros((Q_table2_states_length,Q_table2_actions_length))\n",
    "Q_table2_real=pd.DataFrame(Q_table2_real,columns=Q_table2_action,index=Q_table2_states)\n",
    "Q_table2_real.head()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Set_destination(x,y):\n",
    "    return x,y\n",
    "# Destination_x,Destination_y=Set_destination(900,900)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Draw_map2 funcion is used to train the first Q-Table2, which allows it to arrive to the destination\n",
    "def Draw_map(Destination_x,Destination_y):\n",
    "    im = Image.new(\"RGB\", size=(1000,1000),color=(0,0,0)) \n",
    "    draw = ImageDraw.Draw(im,mode='RGB')\n",
    "    draw.rectangle((40,40,960,960),(255,255,255),(255,255,255))\n",
    "    draw.ellipse((Destination_x-20,Destination_y-20,Destination_x+20,Destination_y+20), (255,0,0), (255,0,0)) \n",
    "    return im\n",
    "im=Draw_map(900,900)\n",
    "plt.imshow(im)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Initial_Q_Table2(LengthOfActions,LengthOfStates):\n",
    "    Q_table2=np.zeros((LengthOfStates,LengthOfActions))\n",
    "    print('***********************************************************')\n",
    "    print(\"Succeed to initialize Q-table2!\")\n",
    "    print('***********************************************************')\n",
    "    return Q_table2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Robot_destination(Destination_x,Destination_y,Current_x,Current_y,Current_angle):\n",
    "    #Left-negative,180-360 |||| Right-positive,0-180\n",
    "    Delta_x=Destination_x-Current_x\n",
    "    Delta_y=Destination_y-Current_y\n",
    "    Distance=np.sqrt(Delta_x*Delta_x+Delta_y*Delta_y)\n",
    "    Direct_angle=(np.arctan(Delta_y/Delta_x))*180/np.pi\n",
    "    if Delta_x<0:\n",
    "        Direct_angle=Direct_angle+180\n",
    "    True_angle=(Current_angle-Direct_angle)%360\n",
    "#     print(true_angle)\n",
    "    if True_angle>180:\n",
    "        return 360-True_angle,'right',Distance\n",
    "    else:\n",
    "        return True_angle,'left',Distance"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Is_Crash function, in the second map, is used to judge if the robot collides with the black frame\n",
    "def Is_Crash(Current_x,Current_y,im):\n",
    "    Crash=False\n",
    "    Degree=[-150,-120,-90,-60,-30,0,30,60,90,120,150,180]\n",
    "    Dis=np.arange(0,50,5)\n",
    "    for i in Dis:\n",
    "        for j in Degree:\n",
    "            x=Current_x+i*np.cos(j/180*np.pi)\n",
    "            y=Current_y+i*np.sin(j/180*np.pi)\n",
    "            if (im.getpixel((x,y)))==(0,0,0):\n",
    "                Crash=True\n",
    "                break\n",
    "        if Crash==True:\n",
    "                break\n",
    "    return Crash"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Is_arrive(Current_x,Current_y,im):\n",
    "    Arrive=False\n",
    "    Crash=False\n",
    "    Degree=[-150,-120,-90,-60,-30,0,30,60,90,120,150,180]\n",
    "    Distance=np.arange(0,20,5)\n",
    "    for i in Distance:\n",
    "        for j in Degree:\n",
    "            x=Current_x+i*np.cos(j/180*np.pi)\n",
    "            y=Current_y+i*np.sin(j/180*np.pi)\n",
    "            if (im.getpixel((x,y)))==(0,0,0):\n",
    "                Crash=True\n",
    "                break\n",
    "            if (im.getpixel((x,y)))==(255,0,0):\n",
    "                Arrive=True\n",
    "                break\n",
    "        if Arrive==True or Crash==True:\n",
    "                break\n",
    "    return Arrive\n",
    "# print(Is_arrive(500,500))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Random_start function, in the second map, is used to set the beginning point in the white map.\n",
    "def Random_start(im):\n",
    "    Angle=0\n",
    "    x,y=np.random.random(2)*1000\n",
    "    while(Is_Crash(x,y,im)==True or Is_arrive(x,y,im)==True):\n",
    "#         print('Boom')\n",
    "        x,y=np.random.random(2)*1000\n",
    "    return x,y,Angle"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Output_state_index(True_angle,Left_right,Distance):\n",
    "    if 0<=Distance<60:\n",
    "        Distance_level=0\n",
    "    elif 60<=Distance<180:\n",
    "        Distance_level=1\n",
    "    elif 180<=Distance<250:\n",
    "        Distance_level=2\n",
    "    else:\n",
    "        Distance_level=3\n",
    "    Angle_level=True_angle//30\n",
    "    \n",
    "    if Left_right=='right':\n",
    "        Left_right_level=1\n",
    "        State_number=int(Distance_level+24*Left_right_level+(5-Angle_level)*4)\n",
    "    elif Left_right=='left':\n",
    "        Left_right_level=0\n",
    "        Angle_level=True_angle//30\n",
    "        State_number=int(Distance_level+24*Left_right_level+Angle_level*4)\n",
    "    return State_number,Distance_level,Angle_level\n",
    "# State_Number,_,_=Output_state_index(20,'right',299) #180 belongs to 180-210,''right' does not have 180\n",
    "# Q_table2_states[State_Number]\n",
    "    \n",
    "    \n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Choose_action2(Q_Table,Current_state,Action_times):\n",
    "    Epsilon=Epsilon_final+(Epsilon_start-Epsilon_final)*np.exp(-1*Decay_rate*Action_times)\n",
    "    State_action=Q_Table[Current_state,:]\n",
    "    if(np.random.random()<Epsilon or np.all(State_action==[0])):\n",
    "        Next_action=np.random.randint(Q_table2_actions_length)\n",
    "    else:\n",
    "        Next_action=np.argmax(State_action)\n",
    "    return Next_action\n",
    "# print(Choose_action2(Q_Table2,20,0))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Output_next_state(Current_x,Current_y,Current_angle,Destination_x,Destination_y,Action,im):\n",
    "    Arrive=False\n",
    "    Crash=False\n",
    "    if Action==0:\n",
    "        Next_x=Current_x+Velocity_tripod*np.cos(Current_angle/180*np.pi)\n",
    "        Next_y=Current_y+Velocity_tripod*np.sin(Current_angle/180*np.pi)\n",
    "        Next_angle=Current_angle\n",
    "        if Is_Crash(Next_x,Next_y,im)==True:\n",
    "            Crash=True\n",
    "        if Is_arrive(Next_x,Next_y,im)==True:\n",
    "            Arrive=True\n",
    "    elif Action==1:\n",
    "        Next_x=Current_x\n",
    "        Next_y=Current_y\n",
    "        Next_angle=Current_angle-45\n",
    "    elif Action==2:\n",
    "        Next_x=Current_x\n",
    "        Next_y=Current_y\n",
    "        Next_angle=Current_angle+45        \n",
    "    True_angle,Left_right,Distance=Robot_destination(Destination_x,Destination_y,Next_x,Next_y,Next_angle)\n",
    "    State_number,Distance_level,Angle_level=Output_state_index(True_angle,Left_right,Distance)\n",
    "    if Arrive==False:\n",
    "        Reward=Distance_level*-0.5-Angle_level\n",
    "    elif Arrive==True:\n",
    "        Reward=50\n",
    "    return Next_x,Next_y,Next_angle,Reward,State_number,Arrive,Crash\n",
    "#Next_state_F2(600,600,50,1800,1800,1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Movement_plot(Vec_x,Vec_y,Destination_x,Destination_y):\n",
    "    im=Image.new(\"RGB\", size=(1000,1000),color=(0,0,0))\n",
    "    draw=ImageDraw.Draw(im,mode='RGB')\n",
    "    draw.rectangle((40,40,960,960),255,255)\n",
    "    draw.ellipse((Destination_x-20,Destination_y-20,Destination_x+20,Destination_y+20),(255,0,0),(255,0,0))\n",
    "    for i,j in zip(Vec_x,Vec_y):\n",
    "        draw.ellipse((i-40,j-40, i+40,j+40),150,150) \n",
    "    return im\n",
    "\n",
    "# plt.imshow(im)\n",
    "# plt.show()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Q_Table2=Initial_Q_Table2(Q_table2_actions_length,Q_table2_states_length)\n",
    "global Epoche\n",
    "Epoche=0\n",
    "global Action_times #Rigister the totality of the times of selecting actions, including the random selections and selection based on Q_Table\n",
    "Action_times=0\n",
    "global Epoche_action_interval"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Q_table2=np.loadtxt(r\"E:\\Graduate\\python\\Q_Table_notgait_finish\\Q_Table2_notgait\\Q_Table2_notgait_2.txt\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib qt5\n",
    "def Draw_result(Destination_x,Destination_y,Start_x,Start_y,Start_angle,Q_table):\n",
    "    Path_x=[]\n",
    "    Path_y=[]\n",
    "    Current_x=Start_x\n",
    "    Current_y=Start_y\n",
    "    Current_angle=Start_angle\n",
    "    Arrive=False\n",
    "    im=Draw_map(Destination_x,Destination_y)\n",
    "    while Arrive==False:\n",
    "        Path_x.append(Current_x)\n",
    "        Path_y.append(Current_y)\n",
    "        True_angle,Left_right,Distance=Robot_destination(Destination_x,Destination_y,\n",
    "                                                             Current_x,Current_y,Current_angle)\n",
    "        Current_state_number,Distance_level,Angle_level=Output_state_index(True_angle,\n",
    "                                                                               Left_right,Distance)\n",
    "        State_action=Q_table2[Current_state_number,:]\n",
    "        Next_action=np.argmax(State_action)\n",
    "        Next_x,Next_y,Next_angle,Reward,Next_state_number,Arrive,Crash=Output_next_state(Current_x,Current_y,\n",
    "                                                                    Current_angle,Destination_x,Destination_y,Next_action,im)\n",
    "        Current_x=Next_x\n",
    "        Current_y=Next_y\n",
    "        Current_angle=Next_angle\n",
    "    im_result=Image.new(\"RGB\", size=(1000,1000),color=(0,0,0))\n",
    "    draw_result=ImageDraw.Draw(im_result,mode='RGB')\n",
    "    draw_result.rectangle((40,40,960,960),(255,255,255),(255,255,255))\n",
    "    draw_result.ellipse((Destination_x-20,Destination_y-20,Destination_x+20,Destination_y+20),(255,0,0),(255,0,0))\n",
    "    print(len(Path_x))\n",
    "    for d in range(36):\n",
    "        for i,j in zip(Path_x[2*d:2*d+2],Path_y[2*d:2*d+2]):\n",
    "            draw_result.ellipse((i-20,j-20, i+20,j+20),(0,255,0),(0,255,0))\n",
    "            plt.imshow(im_result)\n",
    "            plt.savefig((r\"E:\\Graduate\\python\\Q_Table_notgait_finish\\Q_Table2_notgait\\Pic\\picture\")+str(d)+\".png\")\n",
    "    return im_result\n",
    "im_result=Draw_result(800,800,200,200,0,Q_Table2)\n",
    "plt.imshow(im_result)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# np.savetxt(\"Q_Table2_notgait2.txt\",Q_Table2_final)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
